%************************************************
% Verify rpy rotation direction of RTB. RTB's rpy defaults to zyx as the rotation direction
%
% The following example is based on UR5, and the robot pose is that all
% joint angles are 0. 
% 
% From UR5 teach pendant, Base, TCP: 
% x =  -817.25 mm
% y = -191.45 mm
% z = -5.49 mm
% RX = 1.671 rad
% RY = -0.000 rad
% RZ = 0.000 rad
%
% From RTB teach pendant, Base, TCP: 
% x =  -817.250
% y = -191.405
% z = -5.491
% R = -0.0
% P = 0.0
% Y = 90.0
%************************************************

%% rotation vector -> rotation matrix
rotation_vector = [pi/2, 0, 0] % From UR5 teach pendant
axis_angle = [rotation_vector/norm(rotation_vector), norm(rotation_vector)] % rotation vector -> axis-angle
rotation_matrix_by_MATLAB = axang2rotm(axis_angle)

%% rpy -> rotation matrix
rpy_vector = [0, 0, pi/2] % From RTB teach pendant
rotation_matrix_by_RTB = rpy2r(rpy_vector, 'arm')